Restructuring DS402 support. Added MCS2ChanParameters ST and additional FBs to support cleaner and extra operations aside from FB_motionStage & ST_MotionStage by yannS2016 · Pull Request #249 · pcdshub/lcls-twincat-motion

yannS2016 · 2026-01-20T18:32:32Z

Description

https://jira.slac.stanford.edu/browse/ECS-9134

Restructuring DS402 support. Added MCS2ChanParameters ST and additional FBs to support cleaner and extra operations aside from FB_motionStage & ST_MotionStage
Splitting SDO(channel state status, open loop parameter configurations) DS402 MCS2 support into smaller functional blocks

Motivation and Context

DREAM slit and Sample Paddle Support.

How Has This Been Tested?

Tested in the Maker space context with SmartAct MCS2 EtherCAT drive and BSD PLC

Where Has This Been Documented?

Code contains appropriate documentation

Pre-merge checklist

Code works interactively
Test suite passes locally
Code contains descriptive comments
Libraries are set to Always Newest version (Library, *)
Committed with pre-commit or ran pre-commit run --all-files

lcls-twincat-motion/Library/DUTs/DS402/ST_ChannelState.TcDUT

lcls-twincat-motion/Library/DUTs/DS402/ST_MCS2ChanParameter.TcDUT

…al Splits - Ensured stage position is set only on the very first homing request for a stage, preventing cumulative motor/stage offset errors without the need for explicit offset values on the motor record. - Added `pytmc` pragma for MCS2 channelstate raw status. - Removed unused variable/error checks for improved code clarity. - Split SDO (channel state status, open-loop parameter configurations) and DS402 MCS2 support into more modular, smaller functional blocks. - Removed duplicate MCS2 structures and cleaned up legacy mode selection methods. - Restructured DS402 support logic: - Introduced `MCS2ChanParameters` structure. - Added new FBs for modular channel support (in addition to FB_motionStage & ST_MotionStage). - Improved support for reading safety flags and module parameter updates; refactored SDO operations for clarity and maintainability. - Added support for calibrations; split and renamed SDO parameter read & update methods for maintainable operation.

NSLentz

This is really too long to properly review but looking through I don't notice any red flags. The PV name lengths could be reduced to help with our max pv length restrictions.

lcls-twincat-motion/Library/DUTs/DS402/ST_MCS2ChanParameter.TcDUT

yannS2016 · 2026-02-25T00:14:42Z

This is really too long to properly review but looking through I don't notice any red flags. The PV name lengths could be reduced to help with our max pv length restrictions.

yeah, something I've suggested myself, just removed the PLC prefix on properties.

…ge, for ease of testing. moved error reporting to a dedicated FB

KaushikMalapati · 2026-03-12T23:21:22Z

lcls-twincat-motion/Library/Library.plcproj

    <Released>false</Released>
    <Title>lcls-twincat-motion</Title>
-    <ProjectVersion>0.0.0</ProjectVersion>
+    <ProjectVersion>1.0.0</ProjectVersion>


Suggested change

<ProjectVersion>1.0.0</ProjectVersion>

<ProjectVersion>0.0.0</ProjectVersion>

could be. will handle the version in the release stage

KaushikMalapati · 2026-03-12T23:23:07Z

lcls-twincat-motion/Library/Library.plcproj

-    <Compile Include="DUTs\ST_MotionEpicsInterface.TcDUT">
-      <SubType>Code</SubType>
-    </Compile>


Why is this being removed? I don't see any changes to the file

Good catch, before 4.6.0, this was not needed anymore as we decided to create a separate lib for motion abstraction.

lcls-twincat-motion/Library/POUs/Motion/DS402/FB_MotionStageE727.TcPOU

KaushikMalapati · 2026-03-13T00:25:30Z

lcls-twincat-motion/Library/DUTs/DS402/E_MoveDoneCause.TcDUT

+    Unknown := 0,           // Default/uncategorized
+    MoveDone,               // Absolute move completed
+    StepMoveDone,           // Step/JOG move completed
+    HomeDone,               // Homing completed
+    CalibrationDone,        // Calibration completed
+    ResetDone,              // Reset command completed
+    HaltDone,               // Manual halt completed
+    AbortedHome,            // Home aborted
+    AbortedStepMove,        // Step move aborted
+    AbortedCalibration,     // Calibration aborted
+    Referenced,             // Axis became referenced
+    Calibrated              // Axis became calibrated


Should these be capitalized?

KaushikMalapati · 2026-03-13T00:27:10Z

lcls-twincat-motion/Library/DUTs/DS402/ST_MCS2ChanParameter.TcDUT

+    {attribute 'hide'}
+    nChanFErrWinIdx          : WORD;


Why are these hidden?

KaushikMalapati · 2026-03-13T00:27:58Z

lcls-twincat-motion/Library/DUTs/DS402/ST_MCS2ChanParameter.TcDUT

+    // Temporary workaround: Avoid declaring mc_power externally from FB_MotionStage
+    bOverrideDirEnable : BOOL := TRUE;


Is this workaround still needed? I don't see it used anywhere

KaushikMalapati · 2026-03-13T00:29:01Z

lcls-twincat-motion/Library/DUTs/E_EpicsMotorCmd.TcDUT

    HOME := 10,
-    GEAR := 30
+    GEAR := 30,
+    CALIBRATE:=40


Is this used anywhere?

yeah. check the motion stage

KaushikMalapati · 2026-03-13T00:31:53Z

Generally speaking, I think it'd be nice if all the MCS2 function blocks were coupled together somehow so you wouldn't need to instantiate and handle every one manually for every axis in a project.

KaushikMalapati · 2026-03-13T00:33:28Z

lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_WriteStepModeParameters.TcPOU

+    {attribute 'hide'}
+    ftStepUpdateDone    : F_TRIG;
+    tonStartUp : TON;
+    nJogParamStep : INT := 0; // 0=idle/start, 1=steps, 2=amp, 3=freq, 10=done, -1=error


This should be an enum

yeah. i'll turn it into one

Can you also do it for the other FB_Write***Parameters?

KaushikMalapati · 2026-03-13T00:34:44Z

lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_WriteStepModeParameters.TcPOU

+      <ST><![CDATA[IF NOT __ISVALIDREF(stDS402Drive)
+    OR NOT __ISVALIDREF(stMotionStage)
+    OR NOT __ISVALIDREF(stMCS2ChanParameter) THEN
+   RETURN;
+END_IF


Is there any way to automatically perform this check for all function blocks? I see you add it manually often

KaushikMalapati · 2026-03-13T00:35:57Z

lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_WriteStepModeParameters.TcPOU

+nScalededSteps:=LIMIT(-1000, stMCS2ChanParameter.nChanStep, 1000);
+nScalededStepAmp:=REAL_TO_UINT(LIMIT(50, stMCS2ChanParameter.nChanStepAmp, 100) * 655.35);
+nScaledStepFreq:=REAL_TO_UINT(LIMIT(500, stMCS2ChanParameter.nChanStepFreq, 1000.0));


Why do these values have to be limited and should the limits be configurable?

KaushikMalapati · 2026-03-13T00:40:11Z

lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_WriteStepModeParameters.TcPOU

+    tonStartUp(
+        IN := bEnable AND NOT bStepModeParamsSet,
+        PT := T#4S
+    );


Shouldn't this timer be called outside the if bEnable block? If it's inside bEnable=FALSE will never reset it because nScalededSteps won't be called

KaushikMalapati · 2026-03-13T00:42:35Z

lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_WriteStepModeParameters.TcPOU

+    ftStepUpdateDone    : F_TRIG;
+    tonStartUp : TON;
+    nJogParamStep : INT := 0; // 0=idle/start, 1=steps, 2=amp, 3=freq, 10=done, -1=error
+    bForceSequentialSDOs : BOOL := FALSE;


Is this only for debugging? It's never written to

lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_MCS2Calibrate.TcPOU

KaushikMalapati · 2026-03-13T01:50:59Z

lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_MCS2Calibrate.TcPOU

+            // Set bits 0-3 for calibration enable
+            stDS402Drive.nDriveControl.3 := TRUE;


I read the comment as "set bits 0 to 3" but you only set the fourth bit?

I read the comment as "set bits 0 to 3" but you only set the fourth bit?

legacy comment, will be reworded

KaushikMalapati · 2026-03-13T01:52:26Z

lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_MCS2Calibrate.TcPOU

+        END_IF
+
+    E_MoveState.IN_PROGRESS:
+        IF bHalt OR bReset THEN


bHalt and bReset are used in the exact same way so one of them is redundant

KaushikMalapati · 2026-03-13T01:53:38Z

lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_MCS2Calibrate.TcPOU

+            stMotionStage.bError := TRUE;
+            eStepState := E_MoveState.ERROR;
+        ELSE
+            IF tonSettlingTime.Q AND stDS402Drive.stDriveStatus.TargetReached


Why do we need tonSettlingTime if we can always check stDS402Drive.stDriveStatus.TargetReached and why is the settling time constant? Wouldn't different stages have different settling times

lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_MCS2Calibrate.TcPOU

KaushikMalapati · 2026-03-13T02:02:11Z

lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_MCS2ReadParams.TcPOU

Should function block this be removed?

KaushikMalapati · 2026-03-13T02:05:31Z

lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_MCS2StepMode.TcPOU

This is almost exactly identical to FB_MCS2Calibrate. What is the intended difference?

KaushikMalapati · 2026-03-13T02:07:03Z

lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_ReadDriveCodes.TcPOU

+IF ftErrorReadDone.Q AND bEnable THEN
+    IF  NOT fbErrorRead.bError AND nPiezoErrorCode <> 0 THEN
+        stMotionStage.nErrorId := nPiezoErrorCode;
+    END_IF
+    stMotionStage.sErrorMessage := F_MotionErrorCodeLookup(nErrorId:=stMotionStage.nErrorId);
+    fbLogError( stMotionStage:=stMotionStage, bEnable:=stMotionStage.bError);
+ELSE
+    // just return the NC error
+    stMotionStage.sErrorMessage := F_MotionErrorCodeLookup(nErrorId:=stMotionStage.nErrorId);
+    fbLogError( stMotionStage:=stMotionStage, bEnable:=stMotionStage.bError);
+END_IF


This could be written a little simpler by taking the two error lines out of the if block and not having an else case

KaushikMalapati · 2026-03-13T02:09:10Z

lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_ReadDriveCodes.TcPOU

+fbErrorRead(
+    sNetId:= THIS^.stMotionStage.stAxisParameters.sAmsNetId,
+    nSlaveAddr:=stDS402Drive.nSlaveAddr,
+    nIndex:=stMCS2ChanParameter.nChanErrorCodeIdx,
+    nSubIndex :=0,
+    pDstBuf:= ADR(nPiezoErrorCode),
+    cbBufLen:=SIZEOF(nPiezoErrorCode),
+    bExecute:= stMotionStage.bError AND bEnable
+);


Should the timeout input be used so we can't wait indefinitely for this to return?

lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_MCS2DriveManager.TcPOU

…eManager.TcPOU fix doc string syntax Co-authored-by: KaushikMalapati <80156796+KaushikMalapati@users.noreply.github.com>

yannS2016 · 2026-03-13T02:34:36Z

@KaushikMalapati are we missing anything for the screens?

KaushikMalapati · 2026-03-13T02:43:01Z

lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_MCS2Home.TcPOU

+            bReferenced := TRUE;
+            eStepState := E_MoveState.DONE;
+        ELSE
+            bReferenced := FALSE;


It would be nice if we only used homed or only referenced instead of using them interchangeably

KaushikMalapati · 2026-03-13T02:44:18Z

lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_MCS2Home.TcPOU

+    E_MoveState.STARTED:
+        IF NOT stDS402Drive.stDriveStatus.TargetReached
+           AND NOT stDS402Drive.stDriveStatus.Bit13_OpModeSpecific THEN
+            eStepState := E_MoveState.IN_PROGRESS;
+        END_IF


I think we can skip this and just go from INIT to IN_PROGRESS

KaushikMalapati · 2026-03-13T02:52:38Z

lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_MCS2DriveManager.TcPOU

+        1: // RESET FAULT
+            stDS402Drive.nDriveControl := 16#80;
+            tonStartupStep(IN := TRUE);
+            IF tonStartupStep.Q THEN
+                tonStartupStep(IN := FALSE);
+                eStartupState := 2;


I think you should have made a method to set and wait nDriveControl instead of repeating this pattern

KaushikMalapati · 2026-03-13T02:53:47Z

lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_MCS2DriveManager.TcPOU

+    IF bOperational THEN
+        CASE stDS402Drive.nModeOfOperationDisplay OF
+            E_DS402OpMode.CSP:
+                bCSPModeEnabled := TRUE;
+                bHomeModeEnabled := FALSE;
+                bStepModeEnabled := FALSE;
+                bCalibrationModeEnabled := FALSE;
+            E_DS402OpMode.HOME:
+                bCSPModeEnabled := FALSE;
+                bHomeModeEnabled := TRUE;
+                bStepModeEnabled := FALSE;
+                bCalibrationModeEnabled := FALSE;
+            E_DS402OpMode.MCS2_OL_STEP_MODE:
+                bCSPModeEnabled := FALSE;
+                bHomeModeEnabled := FALSE;
+                bStepModeEnabled := TRUE;
+                bCalibrationModeEnabled := FALSE;
+            E_DS402OpMode.MCS2_CALIBRATION:
+                bCSPModeEnabled := FALSE;
+                bHomeModeEnabled := FALSE;
+                bStepModeEnabled := FALSE;
+                bCalibrationModeEnabled := TRUE;
+            ELSE
+                bCSPModeEnabled := FALSE;
+                bHomeModeEnabled := FALSE;
+                bStepModeEnabled := FALSE;
+                bCalibrationModeEnabled := FALSE;
+        END_CASE
+    ELSE
+        bCSPModeEnabled := FALSE;
+        bHomeModeEnabled := FALSE;
+        bStepModeEnabled := FALSE;
+        bCalibrationModeEnabled := FALSE;
+    END_IF


This could be simplified by setting all four booleans false and having a switch where each case just sets a single variable true

KaushikMalapati · 2026-03-13T02:54:29Z

lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_MCS2DriveManager.TcPOU

+    END_IF
+
+    // Mode switch/request evaluator
+    IF bUserExec AND (


Shouldn't this be set to false in the case?

KaushikMalapati · 2026-03-13T02:59:13Z

@KaushikMalapati are we missing anything for the screens?

I believe so, but install your latest branch of this library onto the dream motion and rebuild/restart the ioc so I can double check?

KaushikMalapati · 2026-03-13T03:11:00Z

lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_WriteCalibrationParameters.TcPOU

+    tonStartUp(
+        IN := bEnable AND NOT bCalibrationsParamsSet,
+        PT := T#4S
+    );


Is there something we can poll for instead of waiting for a fixed period of time?

KaushikMalapati · 2026-03-13T03:16:32Z

lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_WriteChanParameters.TcPOU

+        IF ftFErrWinSetDone.Q OR ftSoftLimMinSetDone.Q OR ftSoftLimMaxSetDone.Q OR
+           ftMaxCloseLoopFreqSetDone.Q OR ftSensorPowerModeSetDone.Q OR
+           ftLogicalScaleOffsetSetDone.Q OR ftLogicalScaleInvSetDone.Q THEN


I think every falling trigger might need its own if statement. It doens't make sense that you would set the error based on fbSetFErrorWin if it's not done but any of the other ones are.

KaushikMalapati · 2026-03-13T03:17:33Z

lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_WriteChanParameters.TcPOU

+END_VAR
+]]></Declaration>
+      <Implementation>
+        <ST><![CDATA[nScaledFErrWin:=LREAL_TO_DINT(( stMotionStage.stAxisParameters.fCtrlPosDiffMax / MAX( stMotionStage.stAxisParameters.fEncScaleFactorInternal, stMCS2ChanParameter.fScalingFactor)));


Would you mind explaining why we take the max here and how these two values could be different? In any case, I would store it in variable instead of recalculating for the soft limits

KaushikMalapati · 2026-03-13T06:36:11Z